
/*
*/


#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "rp2040.h"
#include "hardware/resets.h"
#include "hardware/pio.h"
#include "hardware/pio_instructions.h"

#include "umsg.h"
#include "dcchan.h"


/*

从 rp2040 角度看来, read 和 write 都是从输出若干 bit 开始的.
所以写入到 FIFO 

对于 read 操作, 只需要支持 32 位的 dr 寄存器.
因为从 had 模块的 verilog 代码看来, 对于 8 位的 ir 寄存器, read 操作仅仅返回全 1 ???

在 read 动作之前, 需要发送 start,rw,rs 总共 4 个 bits, 这个可以通过 write 操作完成.
如果在之前有 write ir 寄存器, 可以把这 4 个 bits 合并在一起完成.

read 的结果通过 FIFO - output 返回给用户程序, 一共是 2 个 word .
word-0 是读操作采集的 32 bits 寄存器内容.
word-1 只有最低 2 bits 有效, 
bit-0 是采集到的 odd parity, 可以用来判断是否正常.
bit-1 是对应时序中的 SYNC 周期的值, 按照协议应当是 低电平 0 .
这两个 bit 可以用于检查. 对应错误场景是 dut 断电? 线路短路等?

*/

#define dmi2_offset_read_stage 10u
#define dmi2_offset_finish_nty 29u

static const uint16_t dmi2_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block
    0x7201, //  1: out    pins, 1         side 0 [2]
    0x6025, //  2: out    x, 5
    0x6045, //  3: out    y, 5
    0x1929, //  4: jmp    !x, 9           side 1 [1]
    0x0046, //  5: jmp    x--, 6
    0xa142, //  6: nop                           [1]
    0x7401, //  7: out    pins, 1         side 0 [4]
    0x1a46, //  8: jmp    x--, 6          side 1 [2]
    0xa0a2, //  9: mov    pc, y
    0x80a0, // 10: pull   block
    0xf482, // 11: set    pindirs, 2      side 0 [4]
    0xfc01, // 12: set    pins, 1         side 1 [4]
    0x7428, // 13: out    x, 8            side 0 [4]
    0x5901, // 14: in     pins, 1         side 1 [1]
    0xa056, // 15: mov    y, ::isr
    0x0052, // 16: jmp    x--, 18
    0xa242, // 17: nop                           [2]
    0xb442, // 18: nop                    side 0 [4]
    0x5801, // 19: in     pins, 1         side 1
    0x0051, // 20: jmp    x--, 17
    0x8020, // 21: push   block
    0x6028, // 22: out    x, 8
    0x0052, // 23: jmp    x--, 18
    0xb442, // 24: nop                    side 0 [4]
    0x5c01, // 25: in     pins, 1         side 1 [4]
    0x545f, // 26: in     y, 31           side 0 [4]
    0xf803, // 27: set    pins, 3         side 1
    0xe383, // 28: set    pindirs, 3             [3]
    0x8020, // 29: push   block
    0xc000, // 30: irq    nowait 0
    0x0000, // 31: jmp    0
            //     .wrap
};


static const struct pio_program dmi2_program = {
    .instructions = dmi2_program_instructions,
    .length = 32,
    .origin = -1,
};


void PIO0_Q0_Handler( void )
{
    PIO pio = pio0;
    uint sm = 0;
    uint32_t tary[4];
    uint rcnt;

    /* clear */
    pio_interrupt_clear( pio, 0 );
    tary[0] = SYS_PIOS;

    /* get result from fifo */
    rcnt = pio_sm_get_rx_fifo_level( pio, sm );

    if ( rcnt == 3 ) {

        tary[0] = tary[0] | (3ul << 16);
        tary[1] = pio_sm_get( pio, sm );
        tary[2] = pio_sm_get( pio, sm );
        tary[3] = pio_sm_get( pio, sm );

    } else if ( rcnt == 2 ) {

        tary[0] = tary[0] | (2ul << 16);
        tary[1] = pio_sm_get( pio, sm );
        tary[2] = pio_sm_get( pio, sm );
        tary[3] = 0;

    } else if ( rcnt == 1 ) {
        
        tary[0] = tary[0] | (1ul << 16);
        tary[1] = pio_sm_get( pio, sm );
        tary[2] = 0;
        tary[3] = 0;

    } else {

        /* error */
        tary[0] = tary[0] | (0xfffful << 16);
        tary[1] = rcnt;
        tary[2] = 0;
        tary[3] = 0;
    }

    /* send to user */
    sysmsg_send_to_user( 4, tary );
    return;
}


/*
-----------+---------
   A, DIR  |   B
-----------+---------
   8,   9  |  DIO
  10,  11  |  CLK
  12,  13  |  RST
-----------+---------

    side :  [10], CLK
 out, in :   [8], DIO
     set : [8,9], DIO, DIR

*/

static int had_intf_init( uint tck, uint tms, uint32_t freq )
{
    PIO pio = pio0;
    uint sm = 0;
    uint clk_pin = tck;
    uint tms_pin = tms;
    pio_sm_config cfg;

    /* unreset */
    unreset_block_wait( RESETS_RESET_PIO0_BITS );

    /* gpio : tck, dir */
    gpio_pull_up( clk_pin );
    pio_gpio_init( pio, clk_pin );

    gpio_disable_pulls( clk_pin + 1 );
    gpio_set_function( clk_pin + 1, GPIO_FUNC_SIO );
    gpio_set_dir( clk_pin + 1, true );
    gpio_put( clk_pin + 1, true );
    
    /* gpio : tms, dir */
    gpio_pull_up( tms_pin );
    pio_gpio_init( pio, tms_pin );

    gpio_disable_pulls( tms_pin + 1 );
    pio_gpio_init( pio, tms_pin + 1 );

    pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true );
    pio_sm_set_consecutive_pindirs(pio, sm, tms_pin, 2, true );
    pio_sm_set_pins( pio, sm, (1 << clk_pin) | (1 <<tms_pin) | (1 <<(tms_pin + 1)) );

    /* config */
    cfg = pio_get_default_sm_config();
    sm_config_set_out_pins( &cfg, tms_pin, 1 );
    sm_config_set_set_pins( &cfg, tms_pin, 2 );
    sm_config_set_in_pins( &cfg, tms_pin );
    sm_config_set_sideset_pins( &cfg, clk_pin );
    sm_config_set_sideset( &cfg, 2, true, false );
    sm_config_set_clkdiv_int_frac( &cfg, 12, 0 );

    /* INST_MEM */
    pio_add_program_at_offset( pio, &dmi2_program, 0 );

    /**/
    pio_sm_init( pio, sm, 0, &cfg );
    pio_sm_set_enabled( pio, sm, true );


    /* IRQ enable */
    pio_set_irq0_source_enabled( pio, pis_interrupt0, true );
    NVIC_EnableIRQ( PIO0_IRQ_0_IRQn );

    return 0;
}





/*
 cnt : 支持范围 1-22.
bits : 要输出的 bit 序列, 从最低 bit-0 开始串行输出.
 nxt : 执行之后跳转的 pc 值, 支持 3 个值, 0, 10, 25;
*/
static uint32_t had_encode_write( uint cnt, uint32_t bits, uint nxt )
{
    uint32_t t0;

    t0 = bits & 0x1;
    bits = bits >> 1;
    bits = (bits << 5) | nxt;
    bits = (bits << 5) | (cnt - 1);
    bits = (bits << 1) | t0;

    return bits;
}


static int had_enc_null_clock( uint32_t * pdat )
{
    pdat[0] = pdat[1] = pdat[2] = had_encode_write( 22, 0x3FFFFF, 0 );
    pdat[3] = had_encode_write( 22, 0x3FFFFF, dmi2_offset_finish_nty );    
    return 4;
}



static int had_check_parity( uint32_t value, uint32_t parity )
{
    if ( ( (__builtin_popcount(value) + 1) & 0x01 ) != parity ) {
        return -1;
    } else {
        return 0;    
    }
}


static int had_enc_read_ir5( uint32_t * pdat )
{
    uint32_t bits;

    /* rs=10, rw=1, st=0 : 10 1 0 */
    bits = 0b1010;
    pdat[0] = had_encode_write( 4, bits, dmi2_offset_read_stage );
    pdat[1] = 5;
    return 2;
}


static int had_enc_write_ir5( uint8_t idx, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /* trn=1, parity, data=5bits */
    pary = __builtin_popcount( idx ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 5) | (idx & 0x1F );

    /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = (bits << 5) | 0b11000;

    /* 2 + 5 + 5 = 12 */
    pdat[0] = had_encode_write( 12, bits, dmi2_offset_finish_nty );
    return 1;
}


static int had_enc_read_dr32( uint32_t * pdat )
{
    uint32_t bits;

    /* rs=11, rw=1, st=0 : 11 1 0 */
    bits = 0b1110;
    pdat[0] = had_encode_write( 4, bits, dmi2_offset_read_stage );
    pdat[1] = 32;
    return 2;
}


/* 44 bits : addr(10), data(32), op(2)  */
static int had_enc_write_dmi( uint16_t addr, uint32_t data, uint8_t ops, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /* trn, parity */
    pary = __builtin_popcount( addr ) + __builtin_popcount( ops );
    pary = pary + __builtin_popcount( data ) + 1;
    pary = 0x2 | (pary & 1);

    /* data(12), op(2), trn=1, rs=11, rw=0, st=0 : 11100 */
    bits = data & 0xFFF;
    bits = (bits << 2) | ops;
    bits = (bits << 5) | 0b11100;
    pdat[0] = had_encode_write( 19, bits, 0 );

    /* data( high 20 bits ) */
    pdat[1] = had_encode_write( 20, (data >> 12), 0 );

    /* trn=1, parity, addr(10 bits) */
    bits = pary;
    bits = (bits << 10) | addr;
    pdat[2] = had_encode_write( 12, bits, dmi2_offset_finish_nty );;

    return 3;
}



static int had_enc_read_dmi( uint32_t * pdat )
{
    uint32_t bits;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = 0b1110;
    pdat[0] = had_encode_write( 4, bits, dmi2_offset_read_stage );

    /*
    data low 16 + op(2) = 18
    addr(10) + data high 16 = 26
    */
    bits = 26;
    bits = (bits << 8) | 18;
    pdat[1] = bits;

    return 2;
}



#if 0


static int had_enc_write_by_index( uint8_t idx, uint32_t vdr, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /**/
    pary = __builtin_popcount( idx ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | idx;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = had_encode_write( 15, bits, 0 );
    pdat[0] = bits;

    /**/
    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */
    bits = had_encode_write( 21, bits, 0 );
    pdat[1] = bits;

    /**/
    pary = __builtin_popcount( vdr ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 16) | (vdr >> 16);
    bits = had_encode_write( 18, bits, had3_offset_finish_nty );
    pdat[2] = bits;    
    
    /**/
    return 3;
}


static int had_enc_read_by_index( uint8_t idx, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = 0xE;
    pary = __builtin_popcount( idx ) + 1;
    bits = (bits << 2) | 0x2 | (pary & 0x1);
    bits = (bits << 8) | idx;

    /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = (bits << 5) | 0x18;
    bits = had_encode_write( 19, bits, had3_offset_read_stage );

    /**/
    *pdat = bits;
    return 1;
}


static int had_enc_write_dr32( uint32_t vdr, uint32_t * pdat )
{
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);

    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */

    pdat[0] = had_encode_write( 21, bits, 0 );
    pdat[1] = had_encode_write( 18, pary, had3_offset_finish_nty );

    /**/
    return 2;
}


int had_ir8_write( uint8_t vir )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vir ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */

    /**/
    bits = had_encode_write( 15, bits, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_write( uint32_t vdr )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);

    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */

    bits = had_encode_write( 21, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    pary = had_encode_write( 18, pary, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, pary );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_read( uint32_t *pvdr )
{
    PIO pio = pio0;
    int sm = 0;     
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = had_encode_write( 4, 0xE, had3_offset_read_stage );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = pio_sm_get_blocking( pio, sm );
    pary = pio_sm_get_blocking( pio, sm );

    /* check parity */
    if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
        *pvdr = bits;
        return (int)(pary + 0x100);
    }

    /* ok, end */
    *pvdr = bits;
    return 0;
}


int had_dr32_write_by_index( uint8_t vir, uint32_t vdr )
{
    /**/
    PIO pio = pio0;
    int sm = 0;
    uint32_t bits;
    uint32_t pary;

    /**/
    pary = __builtin_popcount( vir ) + 1;
    bits = 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;
    bits = (bits << 5) | 0x18;          /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = had_encode_write( 15, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = vdr & 0xFFFF;
    bits = (bits << 5) | 0x1C;      /* trn=1, rs=11, rw=0, st=0 : 11100 */
    bits = had_encode_write( 21, bits, 0 );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    pary = __builtin_popcount( vdr ) + 1;
    pary = 0x2 | (pary & 0x1);
    pary = (pary << 16) | (vdr >> 16);
    pary = had_encode_write( 18, pary, had3_offset_finish_nty );
    pio_sm_put_blocking( pio, sm, pary );

    /**/
    pio_sm_get_blocking( pio, sm );
    return 0;
}


int had_dr32_read_by_index( uint8_t vir, uint32_t *pvdr )
{
    /**/
    PIO pio = pio0;
    int sm = 0;
    uint32_t bits;
    uint32_t pary;

    /* rs=11, rw=1, st=0 : 1110 */
    bits = 0xE;
    pary = __builtin_popcount( vir ) + 1;
    bits = (bits << 2) | 0x2 | (pary & 0x1);
    bits = (bits << 8) | vir;

    /* trn=1, rs=10, rw=0, st=0 : 11000 */
    bits = (bits << 5) | 0x18;          
    bits = had_encode_write( 19, bits, had3_offset_read_stage );
    pio_sm_put_blocking( pio, sm, bits );

    /**/
    bits = pio_sm_get_blocking( pio, sm );
    pary = pio_sm_get_blocking( pio, sm );

    /* check parity */
    if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
        return (int)(pary + 0x100);
    }

    /* ok, end */
    *pvdr = bits;
    return 0;
}

#endif




/**/
static had_async_session_t * volatile sess_phead = NULL;
static had_async_session_t * volatile sess_ptail = NULL;


static void had_async_to_pio( int tnum, uint32_t * preq )
{
    PIO pio = pio0;
    uint smx = 0;
    
    /**/
    for (int i=0; i<tnum; i++ ) {
        pio_sm_put_blocking( pio, smx, preq[i] );
    }

    return;
}


had_async_session_t * had_async_alloc_session_extra( int autofree, async_resp_cbkf cbkf, int exsz )
{
    had_async_session_t * psess;

    /**/
    psess = (had_async_session_t *)malloc( sizeof(had_async_session_t) + exsz );
    if ( psess == NULL ) {
        return NULL;
    }

    /**/
    psess->next = NULL;
    psess->autofree = autofree;
    psess->req_num = 0;
    psess->cbkf = cbkf;
    psess->parg = NULL;
    psess->step = 0;

    /**/
    return psess;
}


int had_async_alloc_session( async_resp_cbkf cbkf, void * parg, int autofree, had_async_session_t ** ppsess )
{
    had_async_session_t * psess;

    /**/
    psess = (had_async_session_t *)malloc( sizeof(had_async_session_t) );
    if ( psess == NULL ) {
        return 1;
    }

    /**/
    psess->autofree = autofree;
    psess->req_num = 0;
    psess->cbkf = cbkf;
    psess->parg = parg;
    psess->step = 0;

    /**/
    *ppsess = psess;
    return 0;
}


int had_async_post_requst( had_async_session_t * psess, int tnum, uint32_t * preq )
{
    /**/
    psess->next = NULL;    
    psess->req_num = tnum;
    for ( int i=0; i<tnum; i++ ) {
        psess->req_ary[i] = preq[i];
    }

    /* add to list tail */
    if ( sess_ptail == NULL ) {

        /**/
        sess_phead = psess;
        sess_ptail = psess;

        /**/
        had_async_to_pio( tnum, preq );
        psess->stats = 1;

    } else {

        sess_ptail->next = psess;
        sess_ptail = psess;

    }
    
    return 0;
}


/*
用于在 回调函数中, 更新请求数据.
*/
int had_async_refresh_requst( had_async_session_t * psess, int tnum, uint32_t * preq )
{
    /**/
    psess->next = NULL;
    psess->req_num = tnum;
    for ( int i=0; i<tnum; i++ ) {
        psess->req_ary[i] = preq[i];
    }

    /**/
    return 0;
}


int had_async_refresh_callbk( had_async_session_t * psess, async_resp_cbkf cbkf, void * parg )
{
    psess->cbkf = cbkf;
    psess->parg = parg;

    /**/
    return 0;
}


int had_async_proc_resp( int tnum, uint32_t * prsp )
{
    int iret;
    had_async_session_t * psess;

    /**/
    if ( sess_phead == NULL ) {
        /* error, nothing */
        return 1;
    }

    /*
     * 回调函数的返回值, 可以有三种意义: 
     * == 0 : 表示会话完成, 可以删除释放 session 对象.
     *  < 0 : 会话需要继续, 并且放在队列开头, 适用于紧急的会话过程.
     *  > 0 : 会话需要继续, 并且放在队列末尾, 适用于慢速或者轮询状态的会话过程.
     */
    psess = sess_phead;
    iret = psess->cbkf( psess, tnum, prsp );
    __compiler_memory_barrier();

    if ( iret == 0 ) {

        /* free session */
        sess_phead = psess->next;
        if ( psess->autofree ) {
            free( psess );
        }

        /* try next session */
        if ( sess_phead == NULL ) {
            sess_ptail = NULL;
            return 0;
        }

    } else if ( iret > 0 ) {

        /* recycle to tail */
        if ( sess_phead != sess_ptail ) {
            sess_phead = psess->next;
            
            psess->next = NULL;
            sess_ptail->next = psess;
            sess_ptail = psess;
        }

    } else {

        /* nothing, already at head */
    }

    /**/
    psess = sess_phead;
    had_async_to_pio( psess->req_num, psess->req_ary );
    psess->stats += 1;
    return 0;
}


static void had_util_umsg_iret( umsg_t * pmsg, uint32_t iret )
{
    umsg_reset_ofs( pmsg );
    umsg_add_i32( pmsg, iret );
    umsg_send( pmsg );
    return;
}


static int had_cbk_null_clock( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)psess->parg;

    /**/
    had_util_umsg_iret( pmsg, 0 );
    return 0;
}


void had_umsg_null_clock( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_null_clock, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_enc_null_clock( tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_ir5_read( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;
    uint32_t bits;

    /**/
    pmsg = (umsg_t *)psess->parg;
    
    if ( tnum != 2 ) {
        had_util_umsg_iret( pmsg, -2 );
        return 0;
    }

    /* check parity */

    /* succ msg */
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, (prsp[0] >> 27) );
    umsg_add_u32( pmsg, prsp[1] );
    umsg_send( pmsg );
    return 0;
}


void had_umsg_ir5_read( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_ir5_read, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_enc_read_ir5( tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_ir5_write( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    umsg_t * pmsg;
    uint32_t bits;

    /**/
    pmsg = (umsg_t *)psess->parg;
    
    if ( tnum != 1 ) {
        had_util_umsg_iret( pmsg, -2 );
        return 0;
    }

    if ( prsp[0] != 0 ) {
        had_util_umsg_iret( pmsg, -3 );
        return 0;
    }

    /* succ msg */
    had_util_umsg_iret( pmsg, 0 );
    return 0;
}


void had_umsg_ir5_write( umsg_t * pmsg )
{
    int iret;
    uint8_t idx;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_ir5_write, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    idx = umsg_get_u8( pmsg );
    iret = had_enc_write_ir5( idx, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_dr32_read( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    umsg_t * pmsg;
    uint32_t bits;

    /**/
    pmsg = (umsg_t *)psess->parg;
    
    if ( tnum != 2 ) {
        had_util_umsg_iret( pmsg, -2 );
        return 0;
    }

    /* check parity */
    iret = had_check_parity( prsp[0], prsp[1] );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -3 );
        return 0;
    }

    /* succ msg */
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, prsp[0] );
    umsg_add_u32( pmsg, prsp[1] );
    umsg_send( pmsg );
    return 0;
}


void had_umsg_dr32_read( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_dr32_read, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_enc_read_dr32( tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int dmi_cbk_sync( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;


    /**/
    pmsg = (umsg_t *)psess->parg;

    switch ( psess->step ) {
    case 0:
        if ( (tnum != 1) || ( prsp[0] != 0 ) ) {
            had_util_umsg_iret( pmsg, -2 );
            return 0;
        }

        /**/
        iret = had_enc_write_ir5( 0x01, tary );
        had_async_refresh_requst( psess, iret, tary );
        iret = -1;

        /**/
        psess->step += 1;
        break;
    
    case 1:
        /**/
        if ( (tnum != 1) || ( prsp[0] != 0 ) ) {
            had_util_umsg_iret( pmsg, -3 );
            return 0;
        }

        /**/
        iret = had_enc_read_dr32( tary );
        had_async_refresh_requst( psess, iret, tary );
        iret = -1;

        /**/
        psess->step += 1;
        break;

    case 2:
        if ( tnum != 2 ) {
            had_util_umsg_iret( pmsg, -4 );
            return 0;
        }

        /* check parity */
        iret = had_check_parity( prsp[0], prsp[1] );
        if ( iret != 0 ) {

            umsg_reset_ofs( pmsg );
            umsg_add_i32( pmsg, -5 );
            umsg_add_u32( pmsg, prsp[0] );
            umsg_add_u32( pmsg, prsp[1] );
            umsg_send( pmsg );
            
            //had_util_umsg_iret( pmsg, -5 );
            return 0;
        }

        /* save IDCODE */
        pmsg->next = (void *)(uintptr_t)prsp[0];

        /**/
        iret = had_enc_write_ir5( 0x11, tary );
        had_async_refresh_requst( psess, iret, tary );
        iret = -1;

        /**/
        psess->step += 1;
        break;

    case 3:

        /**/
        if ( (tnum != 1) || ( prsp[0] != 0 ) ) {    
            had_util_umsg_iret( pmsg, -6 );
            return 0;
        }

        /**/
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0 );
        umsg_add_u32( pmsg, (uint32_t)(uintptr_t)pmsg->next );
        umsg_send( pmsg );

        /* finish */
        iret = 0;
        break;

    default:
        had_util_umsg_iret( pmsg, -7 );
        iret = 0;
        break;
    }

    /**/
    return iret;
}



/*
发送 null clock.
设置 ir5=0x01, 读 dr32 获取 IDCODE.
设置 ir5=0x11, 返回 succ.
*/
void dmi_umsg_sync( umsg_t * pmsg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( dmi_cbk_sync, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    iret = had_enc_null_clock( tary );
    had_async_post_requst( psess, iret, tary );
    return;
}



#if 0

/*
在内部 DMI 和 DM 之间的 apb 总线上, paddr 使用了 12 bits.
因为都是 4 byts 对齐的读写操作, 所以只有其中高 10 bits 有意义.
再者, 在 e906 上面, 所有 DM 寄存器的地址只使用了 7 个 bits,  0x00 -- 0x7F .
所以下面函数定义的 addr 部分, 使用了 uint8_t 类型, 对于 e906 是足够用了.
*/

static int had_cbk_dm_read( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;
    uint32_t bits;
    uint32_t data;
    uint32_t temp;


    pmsg = (umsg_t *)psess->parg;

    /**/
    switch ( psess->step ) {
    
    case 0:
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 1:
        if ( tnum != 3 ) {
            had_util_umsg_iret( pmsg, -3 );
            iret = 0;
            break;
        }

        /* data low (16), op (2) */
        bits = prsp[0] >> 14;

#if 0
        /* check status ?? */
        if ( (bits & 0x3) == 3 ) {

            iret = had_enc_read_dmi( tary );
            had_async_refresh_requst( psess, iret, tary );
            iret = -1;
            break;
        }
#endif

        /* addr(10), data high (16) */
        temp = prsp[1] >> 6;

        /**/
        data = temp & 0xFFFF;
        data = (data << 16) | (bits >> 2);
        bits = bits & 0x3;
        bits = (bits << 16) | (temp >> 16);

        /**/
        umsg_reset_ofs( pmsg );
        umsg_add_u32( pmsg, bits );
        umsg_add_u32( pmsg, data );
        umsg_add_u32( pmsg, prsp[2] );
        umsg_send( pmsg );
        iret = 0;
        break;

    default:
        had_util_umsg_iret( pmsg, -100 );
        iret = 0;
        break;
    }

    /**/
    return iret;
}

void had_umsg_dm_read_bak( umsg_t * pmsg )
{
    int iret;
    uint8_t idx;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_dm_read, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    idx = umsg_get_u8( pmsg );
    iret = had_enc_write_dmi( idx, 0, 0b01, tary );
    had_async_post_requst( psess, iret, tary );
    return;
}


static int had_cbk_dm_write( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    umsg_t * pmsg;
    uint32_t bits;
    uint32_t data;
    uint32_t temp;


    pmsg = (umsg_t *)psess->parg;

    /**/
    switch ( psess->step ) {
    
    case 0:
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );
        psess->step += 1;
        iret = -1;
        break;

    case 1:
        if ( tnum != 3 ) {
            had_util_umsg_iret( pmsg, -3 );
            iret = 0;
            break;
        }

        /* data low (16), op (2) */
        bits = prsp[0] >> 14;

#if 0
        /* check status ?? */
        if ( (bits & 0x3) == 3 ) {

            iret = had_enc_read_dmi( tary );
            had_async_refresh_requst( psess, iret, tary );
            iret = -1;
            break;
        }
#endif

        /* addr(10), data high (16) */
        temp = prsp[1] >> 6;

        /**/
        data = temp & 0xFFFF;
        data = (data << 16) | (bits >> 2);
        bits = bits & 0x3;
        bits = (bits << 16) | (temp >> 16);

        /**/
        umsg_reset_ofs( pmsg );
        umsg_add_u32( pmsg, bits );
        umsg_add_u32( pmsg, data );
        umsg_add_u32( pmsg, prsp[2] );
        umsg_send( pmsg );
        iret = 0;
        break;

    default:
        had_util_umsg_iret( pmsg, -100 );
        iret = 0;
        break;
    }

    /**/
    return iret;
}


void had_umsg_dm_write_bak( umsg_t * pmsg )
{
    int iret;
    uint8_t idx;
    uint32_t data;
    uint32_t tary[4];
    had_async_session_t * psess;

    iret = had_async_alloc_session( had_cbk_dm_write, (void *)pmsg, true, &psess  );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /**/
    idx = umsg_get_u8( pmsg );
    data = umsg_get_u32( pmsg );
    iret = had_enc_write_dmi( idx, data, 0b10, tary );
    had_async_post_requst( psess, iret, tary );    
    return;
}

#endif


#define DM_data_0       0x04
#define DM_control      0x10
#define DM_status       0x11
#define DM_hartinfo     0x12

#define DM_abs_css      0x16
#define DM_abs_cmd      0x17
#define DM_abs_auto     0x18
#define DM_next_mod     0x1D

#define DM_progbuf0     0x20
#define DM_progbuf1     0x21

#define DM_halt_sum0    0x40
#define DM_comp_id      0x7F


typedef void (* dmi_rdwr_cbkf)( void * parg, uint32_t status, uint32_t data );

typedef struct _tag_dmi_args {
    dmi_rdwr_cbkf cbkf;
    void * parg;
    int step;
} dmi_cbk_args_t;


static int dmi_cbk_rdwr( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    uint32_t bits;
    uint32_t temp;
    uint32_t data;
    uint32_t ops;
    dmi_cbk_args_t * rwarg;

    /**/
    rwarg = (dmi_cbk_args_t *)(psess + 1);

    /**/
    if ( rwarg->step == 0 ) {

        /* delay */
        for ( iret=0; iret < 10000; iret+=1 ) {
            __asm volatile ( "nop" );
        }

        /* post */
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );
        rwarg->step += 1;

        /* not free, add to head */
        return -1;
    }

    /* 读操作一定会返回 3 个 word 数据. */
    if ( tnum != 3 ) {
        /* call back */
        rwarg->cbkf( rwarg->parg, 0xff0000 + rwarg->step, tnum );
        return 0;
    }

    /* data low (16), op (2) */
    bits = prsp[0] >> 14;
    data = __builtin_popcount( bits );

    /* addr(10), data high (16) */
    temp = prsp[1] >> 6;
    data = data + __builtin_popcount( temp );

    /* check resp bits, parity */
    if ( ((data + 1) & 0x01) != prsp[2] )  {
        /* parity error, call back */
        rwarg->cbkf( rwarg->parg, 0xFF02, prsp[2] );
        return 0;
    }

    /*
    整理格式:
    其中 bits 包含 ops(2),  addr(16) ,  其实 addr 中仅有 10 bits 有效.
    其中 data 包含读返回的 32 位数据.
    */
    data = temp & 0xFFFF;
    data = (data << 16) | (bits >> 2);
    ops = bits & 0x3;
    bits = (ops << 16) | (temp >> 16);

    
    if ( ops == 0x3 )  {
        
        if ( rwarg->step > 16 )  {
            /**/
            rwarg->cbkf( rwarg->parg, bits, data );
            return 0;
        }

        /* delay */
        for ( iret=0; iret < 10000; iret+=1 ) {
            __asm volatile ( "nop" );
        }

        /* 如果操作没有完成, 需要重新发起 read 操作. */
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );
        rwarg->step += 1;

        /* not free, add to head */
        return -1;
    }

    /**/
    rwarg->cbkf( rwarg->parg, bits, data );
    return 0;
}


int dmi_read_async( uint8_t idx, dmi_rdwr_cbkf cbkf, void * parg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;
    dmi_cbk_args_t * rwarg;

    psess = had_async_alloc_session_extra( true, dmi_cbk_rdwr, sizeof(dmi_cbk_args_t) );
    if ( psess == NULL ) {
        return -1;
    }

    /**/
    rwarg = (dmi_cbk_args_t *)(psess + 1);
    rwarg->cbkf = cbkf;
    rwarg->parg = parg;
    rwarg->step = 0;

    /* 0b01 : read action */
    iret = had_enc_write_dmi( idx, 0, 0b01, tary );
    had_async_post_requst( psess, iret, tary );    
    return 0;
}


int dmi_write_async( uint8_t idx, uint32_t data, dmi_rdwr_cbkf cbkf, void * parg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;
    dmi_cbk_args_t * rwarg;

    psess = had_async_alloc_session_extra( true, dmi_cbk_rdwr, sizeof(dmi_cbk_args_t) );
    if ( psess == NULL ) {
        return -1;
    }

    /**/
    rwarg = (dmi_cbk_args_t *)(psess + 1);
    rwarg->cbkf = cbkf;
    rwarg->parg = parg;
    rwarg->step = 0;

    /* 0b10 : write action */
    iret = had_enc_write_dmi( idx, data, 0b10, tary );
    had_async_post_requst( psess, iret, tary );
    return 0;
}


/*
检查 dmi read 操作之后的返回结果.
同时整理和对齐数据..
*/
static int util_check_resp( uint32_t * prsp, uint8_t addr, uint32_t * puv )
{
    uint32_t bits;
    uint32_t data;
    uint32_t temp;
    uint32_t parity;

    /* data low (16), op (2) */
    bits = prsp[0] >> 14;
    parity = __builtin_popcount( bits );

    /* addr(10), data high (16) */
    temp = prsp[1] >> 6;
    parity = parity + __builtin_popcount( temp );

    /* low(1), parity(1) */
    if ( ((parity + 1) & 0x01) != prsp[2] )  {
        return 111;
    }

    /**/
    data = temp & 0xFFFF;
    data = (data << 16) | (bits >> 2);

    bits = bits & 0x3;
    bits = (bits << 16) | (temp >> 16);

    /*
    op 两个 bits 表示前一次操作的状态, 0b00 表示正确完成. 
    所以这里判断 addr 部分 10 个 bits, 必须跟上次操作的 addr 部分相同.
    */
    if ( bits != (uint32_t)addr ) {
        return 222;
    }

    /**/
    *puv = data;
    return 0;
}



static int dmi_cbk_abscmd( had_async_session_t * psess, int tnum, uint32_t * prsp )
{
    int iret;
    uint32_t tary[4];
    uint32_t bits;
    uint32_t temp;
    uint32_t data;
    dmi_cbk_args_t * rwarg;

    /**/
    rwarg = (dmi_cbk_args_t *)(psess + 1);

    /**/
    switch ( rwarg->step ) {
    
    case 0:
        /* post */
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );

        /* not free, add to head */
        rwarg->step += 1;
        iret = -1;
        break;

    case 1:

        if ( tnum != 3 ) {
            rwarg->cbkf( rwarg->parg, 0xE000001, tnum );
            return 0;
        }

        /* check parity and check address */
        iret = util_check_resp( prsp, DM_abs_cmd, &data );
        if ( iret != 0 ) {
            rwarg->cbkf( rwarg->parg, 0xE010000 + iret, data );
            return 0;
        }

        /* abs_status, op=0b01, read */
        iret = had_enc_write_dmi( DM_abs_css, 0, 0b01, tary );
        had_async_refresh_requst( psess, iret, tary );

        /* not free, add to head */
        rwarg->step += 1;
        iret = -1;
        break;

    case 2:
        /* post */
        iret = had_enc_read_dmi( tary );
        had_async_refresh_requst( psess, iret, tary );

        /* not free, add to head */
        rwarg->step += 1;
        iret = -1;
        break;

    case 3:
        if ( tnum != 3 ) {
            rwarg->cbkf( rwarg->parg, 0xE020000, tnum );
            return 0;
        }

        /* check parity and check address */
        iret = util_check_resp( prsp, DM_abs_css, &data );
        if ( iret != 0 ) {
            rwarg->cbkf( rwarg->parg, 0xE030000 + iret, data );
            return 0;
        }

        /* check busy and err  */
        if ( (data & 0x1700) != 0 ) {
            rwarg->cbkf( rwarg->parg, 0xE040000, data );
            return 0;
        }

        /* success */
        rwarg->cbkf( rwarg->parg, DM_abs_cmd, data );
        return 0;

    default:
        rwarg->cbkf( rwarg->parg, 0xE050000, data );
        return 0;
    }

    /**/
    return iret;

}


/**/
int dmi_abscmd_async( uint32_t acmd, dmi_rdwr_cbkf cbkf, void * parg )
{
    int iret;
    uint32_t tary[4];
    had_async_session_t * psess;
    dmi_cbk_args_t * rwarg;

    psess = had_async_alloc_session_extra( true, dmi_cbk_abscmd, sizeof(dmi_cbk_args_t) );
    if ( psess == NULL ) {
        return -1;
    }

    /**/
    rwarg = (dmi_cbk_args_t *)(psess + 1);
    rwarg->cbkf = cbkf;
    rwarg->parg = parg;
    rwarg->step = 0;

    /* abs_cmd, op=0b10, write */
    iret = had_enc_write_dmi( DM_abs_cmd, acmd, 0b10, tary );
    had_async_post_requst( psess, iret, tary );
    return 0;
}



void had_cbk_dmi_read( void * parg, uint32_t status, uint32_t data )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)parg;

    /**/
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, status );
    umsg_add_u32( pmsg, data );
    umsg_send( pmsg );
    return;
}


void had_umsg_dm_read( umsg_t * pmsg )
{
    int iret;
    uint8_t idx;

    /**/
    idx = umsg_get_u8( pmsg );
    iret = dmi_read_async( idx, had_cbk_dmi_read, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
    }

    return;
}


void had_cbk_dmi_write( void * parg, uint32_t status, uint32_t data )
{
    umsg_t * pmsg;

    /**/
    pmsg = (umsg_t *)parg;

    /**/
    umsg_reset_ofs( pmsg );
    umsg_add_u32( pmsg, status );
    umsg_add_u32( pmsg, data );
    umsg_send( pmsg );
    return;
}


void had_umsg_dm_write( umsg_t * pmsg )
{
    int iret;
    uint8_t idx;
    uint32_t data;

    /**/
    idx = umsg_get_u8( pmsg );
    data = umsg_get_u32( pmsg );
    iret = dmi_write_async( idx, data, had_cbk_dmi_write, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -1 );
    }
    return;
}



void dmi_cbk_write_mem( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;
    uint32_t temp;

    /**/
    iret = 0;
    pmsg = (umsg_t *)parg;

    switch ( pmsg->rsvd22 )  {
    case 0:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -4;
            break;
        }

        /* prog_buf_0 : CSRR a3, dscratch0 */
        temp = 0x7b2026f3;
        iret = dmi_write_async( DM_progbuf0, temp, dmi_cbk_write_mem, pmsg );
        if ( iret != 0 ) {
            iret = -5;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 1:
        /* check status */
        if ( status != DM_progbuf0 ) {
            iret = -6;
            break;
        }

        /* prog_buf_1 : SWIA a3, (a2), 4, 0 */
        temp = 0x5846568b;
        iret = dmi_write_async( DM_progbuf1, temp, dmi_cbk_write_mem, pmsg );
        if ( iret != 0 ) {
            iret = -7;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 2:
        /* check status */
        if ( status != DM_progbuf1 ) {
            iret = -8;
            break;
        }

        /* abs_cmd : write, regno=100C : a2 = addr */
        temp = 0x0023100C;
        iret = dmi_abscmd_async( temp, dmi_cbk_write_mem, pmsg );
        if ( iret != 0 ) {
            iret = -9;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 3:
        /* check status */
        if ( status != DM_abs_cmd ) {
            iret = -10;
            break;
        }

        /**/
        if ( umsg_remains(pmsg) < 4 ) {
            had_util_umsg_iret( pmsg, 0 );
            iret = 0;
            break;
        }

        /* get array[x] */
        temp = umsg_get_u32( pmsg );
        iret = dmi_write_async( DM_data_0, temp, dmi_cbk_write_mem, pmsg );
        if ( iret != 0 ) {
            iret = -14;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 4:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -16;
            break;
        }

        /* abs_cmd : write, postexec, regno=100D : a3 = array[0] */
        temp = 0x0027100D;
        iret = dmi_abscmd_async( temp, dmi_cbk_write_mem, pmsg );
        if ( iret != 0 ) {
            iret = -17;
        }

        /* goto step-3 */
        pmsg->rsvd22 = 3;
        break;

    default:
        iret = -100;
        break;
    }


    /**/
    if ( iret != 0 )  {
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, iret );
        umsg_add_u32( pmsg, status );
        umsg_add_u32( pmsg, data );
        umsg_add_u8( pmsg, 0xEE );
        umsg_send( pmsg );
    }

    return;
}



/*
     4 bytes : target memory address
remain bytes : bytes to write, max 56 bytes.

在回调函数中, 使用 pmsg 中部分内存(next, rsvd22), 作为上下文来使用.
其他几个字段不要破坏.
好处是可以避免分配额外内存.
*/
void dmi_umsg_write_mem( umsg_t * pmsg )
{
    int iret;
    uint32_t addr;

    /**/
    iret = umsg_remains( pmsg );
    if ( iret < 8 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /* rsvd22, 临时作为状态机的 step 来使用. */
    addr = umsg_get_u32( pmsg );
    pmsg->rsvd22 = 0;
    pmsg->next = NULL;

    /* write addr to DATA-0 */
    iret = dmi_write_async( DM_data_0, addr, dmi_cbk_write_mem, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* ok, wait async step */
    return;
}



void dmi_cbk_read_mem( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;
    uint32_t temp;

    /**/
    iret = 0;
    pmsg = (umsg_t *)parg;

    switch ( pmsg->rsvd22 ) {

    case 0:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -4;
            break;
        }

        /* prog_buf_0 : LWIA a3, (a2), 4, 0 : a3(x13), a2(x12) */
        temp = 0x5846468b;
        iret = dmi_write_async( DM_progbuf0, temp, dmi_cbk_read_mem, parg );
        if ( iret != 0 ) {
            iret = -5;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 1:
        /* check status */
        if ( status != DM_progbuf0 ) {
            iret = -6;
            break;
        }

        /* prog_buf_1 : CSRW a3, 0x7b2 :  */
        temp = 0x7b269073;
        iret = dmi_write_async( DM_progbuf1, temp, dmi_cbk_read_mem, parg );
        if ( iret != 0 ) {
            iret = -7;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 2:
        /* check status */
        if ( status != DM_progbuf1 ) {
            iret = -8;
            break;
        }

        /* abs_cmd : postexec, write, regno=100C : a2 = addr */
        temp = 0x0027100C;
        iret = dmi_abscmd_async( temp, dmi_cbk_read_mem, parg );
        if ( iret != 0 ) {
            iret = -9;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 3:
        
        /* check status */
        if ( status != DM_abs_cmd ) {
            iret = -10;
            break;
        }

        /**/
        if ( pmsg->mlen > 1 ) {

            /* read csr 0x7b2 and post exec */
            /* abs_cmd : postexec, read, regno=0x7b2 */
            temp = 0x002607b2;
            iret = dmi_abscmd_async( temp, dmi_cbk_read_mem, parg );

        } else {

            /* read csr 0x7b2 only */
            /* abs_cmd : read, regno=0x7b2 */
            temp = 0x002207b2;
            iret = dmi_abscmd_async( temp, dmi_cbk_read_mem, parg );

        }

        /**/
        pmsg->mlen -= 1;
        pmsg->rsvd22 += 1;
        break;

    case 4:
        /* check status */
        if ( status != DM_abs_cmd ) {
            iret = -10;
            break;
        }

        /* data-0 : get readed data  */
        iret = dmi_read_async( DM_data_0, dmi_cbk_read_mem, parg );
        if ( iret != 0 ) {
            iret = -11;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 5:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -4;
            break;
        }

        /**/
        umsg_add_u32( pmsg, data );

        /**/
        if ( pmsg->mlen > 1 ) {

            /* read csr 0x7b2 and post exec */
            /* abs_cmd : postexec, read, regno=0x7b2  */
            temp = 0x002607b2;
            iret = dmi_abscmd_async( temp, dmi_cbk_read_mem, parg );
            
            pmsg->mlen -= 1;
            pmsg->rsvd22 = 4;

        } else if ( pmsg->mlen == 1 ) {

            /* read csr 0x7b2 only */
            /* abs_cmd : read, regno=0x7b2  */
            temp = 0x002207b2;
            iret = dmi_abscmd_async( temp, dmi_cbk_read_mem, parg );
            
            pmsg->mlen -= 1;
            pmsg->rsvd22 = 4;

        } else {

            /* finish */
            umsg_send( pmsg );
            pmsg->rsvd22 = 100;

        }

        iret = 0;
        break;

    default:
        iret = -100;
        break;
    }

    /**/
    if ( iret != 0 )  {
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, iret );
        umsg_add_u32( pmsg, status );
        umsg_add_u32( pmsg, data );
        umsg_add_u8( pmsg, 0xEE );
        umsg_send( pmsg );
    }
}

/*
4 bytes : target memory address
 1 byte : count for mem32, 最大 14 = 56 bytes

返回:

如果返回消息长度不是 (count * 4) bytes.
那么构造一个 5-7 字节的消息, 消息中包括错误码 (int32_t), 
*/
void dmi_umsg_read_mem( umsg_t * pmsg )
{
    int iret;
    uint32_t addr;
    uint8_t cntw;

    /**/
    iret = umsg_remains( pmsg );
    if ( iret < 5 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /* rsvd22, 临时作为状态机的 step 来使用. */
    addr = umsg_get_u32( pmsg );
    cntw = umsg_get_u8( pmsg );
    pmsg->rsvd22 = 0;
    pmsg->mlen = cntw;
    pmsg->next = NULL;

    if ( (cntw > 12) || (cntw == 0) ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* write addr to DATA-0 */
    iret = dmi_write_async( DM_data_0, addr, dmi_cbk_read_mem, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* ok, wait async step */
    umsg_reset_ofs( pmsg );
    umsg_add_i32( pmsg, 0 );
    return;
}


static void dmi_cbk_poll_stop( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;

    if ( status != DM_status ) {
        pmsg = umsg_alloc();
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0x100001 );
        umsg_dbg_send( pmsg );
        return;
    }

    if ( (data & 0xF00) == 0x300 ) {
        pmsg = umsg_alloc();
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0x100002 );
        umsg_add_u32( pmsg, data );
        umsg_dbg_send( pmsg );
        return;
    }

    //
    iret = dmi_read_async( DM_status, dmi_cbk_poll_stop, NULL );
    if ( 0 != iret ) {
        pmsg = umsg_alloc();
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0x123477 );
        umsg_dbg_send( pmsg );
    }

    return;
}



static void dmi_cbk_leave_debug( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;
    uint32_t temp;

    /**/
    iret = 0;
    pmsg = (umsg_t *)parg;

    switch ( pmsg->rsvd22 )  {
    
    case 0:
        if ( status != DM_data_0 ) {
            iret = -4;
            break;
        }

        /* abs_cmd : write, regno=0x7B1 */
        temp = 0x002307B1;
        iret = dmi_write_async( DM_abs_cmd, temp, dmi_cbk_leave_debug, parg );
        if ( iret != 0 ) {
            iret = -5;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 1:
        if ( status != DM_abs_cmd ) {
            iret = -6;
            break;
        }

        /* abs_css : check error  */
        iret = dmi_read_async( DM_abs_css, dmi_cbk_leave_debug, parg );
        if ( iret != 0 ) {
            iret = -7;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 2:
        /* check status */
        if ( status != DM_abs_css ) {
            iret = -8;
            break;
        }

        /* busy, cmderr */
        if ( (data & 0x1700) != 0 ) {
            iret = -9;
            break;
        }

        /**/
        temp = 0x40010000;
        iret = dmi_write_async( DM_control, temp, dmi_cbk_leave_debug, parg );
        if ( iret != 0 ) {
            iret = -10;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;        

    case 3:
        /* check status */
        if ( status != DM_control ) {
            iret = -12;
            break;
        }

        /* status : check error  */
        iret = dmi_read_async( DM_status, dmi_cbk_leave_debug, parg );
        if ( iret != 0 ) {
            iret = -13;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 4:
        /* check status */
        if ( status != DM_status ) {
            iret = -14;
            break;
        }

        temp = (uint32_t)(uintptr_t)pmsg->next;
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0 );
        umsg_add_u32( pmsg, data );
        umsg_send( pmsg );

#if 0
        if ( temp != 0 ) {

            /* poll status for stop */
            iret = dmi_read_async( DM_status, dmi_cbk_poll_stop, NULL );
            if ( 0 != iret ) {
                pmsg = umsg_alloc();
                umsg_reset_ofs( pmsg );
                umsg_add_i32( pmsg, 0x123456 );
                umsg_dbg_send( pmsg );
            }
        }
#endif

        /**/
        iret = 0;
        break;
        
    default:
        iret = -100;
        break;
    }

    /**/
    if ( iret != 0 )  {
        had_util_umsg_iret( pmsg, iret );
    }

    return;
}


void dmi_umsg_leave_debug( umsg_t * pmsg )
{
    int iret;
    uint32_t dpc;
    uint32_t flag;

    /* check args */
    iret = umsg_remains( pmsg );
    if ( iret < 8 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /* rsvd22, 临时作为状态机的 step 来使用. */
    dpc = umsg_get_u32( pmsg );
    flag = umsg_get_u32( pmsg );
    pmsg->rsvd22 = 0;
    pmsg->next = (void *)(uintptr_t)flag;

    /**/
    iret = dmi_write_async( DM_data_0, dpc, dmi_cbk_leave_debug, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* ok, wait async step */
    return;
}


void dmi_cbk_cpu_read( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;

    /**/
    iret = 0;
    pmsg = (umsg_t *)parg;

    switch ( pmsg->rsvd22 )  {

    case 0:
        /* check status */
        if ( status != DM_abs_cmd ) {
            iret = -4;
            break;
        }
        
        /* abs_css : check error  */
        iret = dmi_read_async( DM_abs_css, dmi_cbk_cpu_read, parg );
        if ( iret != 0 ) {
            iret = -5;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 1:
        /* check status */
        if ( status != DM_abs_css ) {
            iret = -6;
            break;
        }

        /* busy, cmderr */
        if ( (data & 0x1700) != 0 ) {
            
            umsg_reset_ofs( pmsg );
            umsg_add_i32( pmsg, -7 );
            umsg_add_u32( pmsg, data );
            umsg_send( pmsg );        

            //iret = -7;
            iret = 0;
            break;
        }

        /* read data0 */
        iret = dmi_read_async( DM_data_0, dmi_cbk_cpu_read, parg );
        if ( iret != 0 ) {
            iret = -8;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 2:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -10;
            break;
        }

        /**/
        umsg_reset_ofs( pmsg );
        umsg_add_i32( pmsg, 0 );
        umsg_add_u32( pmsg, data );
        umsg_send( pmsg );        

        /**/
        iret = 0;
        break;

    default:
        iret = -100;
        break;
    }


    /**/
    if ( iret != 0 )  {
        had_util_umsg_iret( pmsg, iret );
    }

    return;
}

/*
使用 abs_cmd 来读取寄存器.
输入参数是 16 bits 的 index.
根据文档的描述来规定 index 的范围和意义.
暂时先不考虑 浮点寄存器 64 bits 的问题, 后续实验测试.
*/
void dmi_umsg_cpu_read( umsg_t * pmsg )
{
    int iret;
    uint16_t ridx;
    uint32_t temp;

    /* check args */
    iret = umsg_remains( pmsg );
    if ( iret < 2 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /* rsvd22, 临时作为状态机的 step 来使用. */
    ridx = umsg_get_u16( pmsg );
    pmsg->rsvd22 = 0;
    pmsg->next = NULL;

    /* abs_cmd : aarsize=2, transfer=1 */
    temp = 0x0022;
    temp = (temp << 16) + ridx;
    iret = dmi_write_async( DM_abs_cmd, temp, dmi_cbk_cpu_read, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* ok, wait async step */
    return;
}




void dmi_cbk_cpu_write( void * parg, uint32_t status, uint32_t data )
{
    int iret;
    umsg_t * pmsg;
    uint16_t ridx;
    uint32_t temp;

    /**/
    iret = 0;
    pmsg = (umsg_t *)parg;

    switch ( pmsg->rsvd22 )  {

    case 0:
        /* check status */
        if ( status != DM_data_0 ) {
            iret = -4;
            break;
        }

        /**/
        ridx = (uint16_t)(uintptr_t)pmsg->next;

        /* abs_cmd : aarsize=2, transfer=1, write=1 */
        temp = 0x0023;
        temp = (temp << 16) | ridx;
        iret = dmi_write_async( DM_abs_cmd, temp, dmi_cbk_cpu_write, parg );
        if ( iret != 0 ) {
            iret = -5;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 1:
        /* check status */
        if ( status != DM_abs_cmd ) {
            iret = -6;
            break;
        }
        
        /* abs_css : check error  */
        iret = dmi_read_async( DM_abs_css, dmi_cbk_cpu_write, parg );
        if ( iret != 0 ) {
            iret = -7;
            break;
        }

        /**/
        pmsg->rsvd22 += 1;
        break;

    case 2:
        /* check status */
        if ( status != DM_abs_css ) {
            iret = -8;
            break;
        }

        /* busy, cmderr */
        if ( (data & 0x1700) != 0 ) {
            iret = -9;
            break;
        }

        /* return msg */
        had_util_umsg_iret( pmsg, 0 );
        break;

    default:
        iret = -100;
        break;
    }

    /**/
    if ( iret != 0 )  {
        had_util_umsg_iret( pmsg, iret );
    }
}



void dmi_umsg_cpu_write( umsg_t * pmsg )
{
    int iret;
    uint16_t ridx;
    uint32_t temp;

    /* check args */
    iret = umsg_remains( pmsg );
    if ( iret < 6 ) {
        had_util_umsg_iret( pmsg, -1 );
        return;
    }

    /* rsvd22, 临时作为状态机的 step 来使用. */
    ridx = umsg_get_u16( pmsg );
    temp = umsg_get_u32( pmsg );
    pmsg->rsvd22 = 0;
    pmsg->next = (void *)(uintptr_t)ridx;

    /* data-0 */
    iret = dmi_write_async( DM_data_0, temp, dmi_cbk_cpu_write, pmsg );
    if ( iret != 0 ) {
        had_util_umsg_iret( pmsg, -2 );
        return;
    }

    /* ok, wait async step */
    return;
}


void dmi_umsg_tclk_set( umsg_t * pmsg )
{
    PIO pio = pio0;
    uint sm = 0;
    uint32_t temp;

    /**/
    temp = umsg_get_u32( pmsg );
    pio_sm_set_clkdiv_int_frac( pio, sm, (uint16_t)temp, 0 );
    pio_sm_clkdiv_restart( pio, sm );
    
    /**/
    had_util_umsg_iret( pmsg, 0 );
    return;
}


void dmi_umsg_tclk_get( umsg_t * pmsg )
{
    PIO pio = pio0;
    uint32_t temp;

    /**/
    temp = pio->sm[0].clkdiv;
    
    /**/
    had_util_umsg_iret( pmsg, temp );
}


int had_mod_init( uint tck, uint tms, uint32_t freq )
{
    int iret;
    
    /**/
    iret = had_intf_init( tck, tms, freq );
    return iret;
}

